/*----------------------------------------------------------------------------*/
/* Team 3647
/* Auton.java
/* Auton Routine
/*----------------------------------------------------------------------------*/
package edu.wpi.first.wpilibj.templates;

import edu.wpi.first.wpilibj.Timer;

public class Auton {

    private static long startTime  = 0;
    private static long feedtime   = 320;
    private static long startfeed1 = 2000;
    private static long startfeed2 = 3500;
    private static long startfeed3 = 6500;
    private static boolean runonce = false;

    public static void resetAuton() {
        runonce = false;
    }

    public static void move() {
        if (!runonce) {
            Loader.CollectorDown();
            Drivetrain.Drive(-.3, -.3);
            Timer.delay(1.7); // Old 3.7 Sec //Odd value needs re-tuning with new gearing 
            Drivetrain.Drive(0, 0);
            runonce = true; 
            startTime = System.currentTimeMillis();
            Timer.delay(.05);
        }
    }

    public static void AutonProcess() {
        
        Shooter.ShooterAuton();
        long elapsed = System.currentTimeMillis() - startTime;
        
        if (elapsed > startfeed1 && elapsed < startfeed1 + (feedtime)) {
            Loader.LoaderFeed();
        } else if (elapsed > startfeed2 && elapsed < (startfeed2 + feedtime)) {
            Loader.LoaderFeed();
        } else if (elapsed > startfeed3) {
            Loader.LoaderFeed();
        } else {
            Loader.LoaderKill();
        }
    }
}
